#include "SharedSettings.h"
#include "CMatrix4dh.h"


const fixed CMatrix4dh::LookAtUpVectorTolerance	= CMatrix4dh_LookAtUpVectorTolerance;


CMatrix4dh::CMatrix4dh()
{
	m_nOfArithmeticOpsSinceLastFixup = 0;
	m_nOfArthmeticOpsTriggeringOrientationFixup = 0; 

	if( m_nOfArthmeticOpsTriggeringOrientationFixup )
		m_orientationFixupIsApplicable = TRUE;
	else
		m_orientationFixupIsApplicable = FALSE;
}


CMatrix4dh::CMatrix4dh( const fixed* m )
{
	ASSERT( m ); 

	MEMCPY( this, m, sizeof( CMatrix4dh ) );

	ASSERT( !m_orientationFixupIsApplicable || 
			( m_orientationFixupIsApplicable && m_nOfArthmeticOpsTriggeringOrientationFixup ) );
}

 
CMatrix4dh::CMatrix4dh( fixed m00, fixed m01, fixed m02,
						fixed m10, fixed m11, fixed m12,
						fixed m20, fixed m21, fixed m22, 
						fixed m30, fixed m31, fixed m32,
						int16 nOfArthmeticOpsTriggeringOrientationFixup )
{ 
	m_col0.m_i = m00; 
	m_col0.m_j = m01;
	m_col0.m_k = m02;
	m_col0.m_w = 0;
	m_col1.m_i = m10; 
	m_col1.m_j = m11;
	m_col1.m_k = m12;
	m_col1.m_w = 0;
	m_col2.m_i = m20; 
	m_col2.m_j = m21;
	m_col2.m_k = m22;
	m_col2.m_w = 0;
	m_col3.m_i = m30; 
	m_col3.m_j = m31;
	m_col3.m_k = m32;
	m_col3.m_w = CMathFixed::One;

	m_nOfArithmeticOpsSinceLastFixup = 0;
	m_nOfArthmeticOpsTriggeringOrientationFixup = nOfArthmeticOpsTriggeringOrientationFixup; 

	if( m_nOfArthmeticOpsTriggeringOrientationFixup )
		m_orientationFixupIsApplicable = TRUE;
	else
		m_orientationFixupIsApplicable = FALSE;
}



CMatrix4dh::CMatrix4dh( const CVector4dh* col, int16 nOfArthmeticOpsTriggeringOrientationFixup )
{ 
	MEMCPY( this, col, sizeof( CVector4dh ) * 4 );

	m_nOfArithmeticOpsSinceLastFixup = 0;
	m_nOfArthmeticOpsTriggeringOrientationFixup = nOfArthmeticOpsTriggeringOrientationFixup; 

	if( m_nOfArthmeticOpsTriggeringOrientationFixup )
		m_orientationFixupIsApplicable = TRUE;
	else
		m_orientationFixupIsApplicable = FALSE;
}


CMatrix4dh::CMatrix4dh( const CVector4dh& col0, const CVector4dh& col1, 
						const CVector4dh& col2, const CVector4dh& col3,
						int16 nOfArthmeticOpsTriggeringOrientationFixup )
{ 
	m_col0 = col0; 
	m_col1 = col1; 
	m_col2 = col2; 
	m_col3 = col3; 

	m_nOfArithmeticOpsSinceLastFixup = 0;
	m_nOfArthmeticOpsTriggeringOrientationFixup = nOfArthmeticOpsTriggeringOrientationFixup; 

	if( m_nOfArthmeticOpsTriggeringOrientationFixup )
		m_orientationFixupIsApplicable = TRUE;
	else
		m_orientationFixupIsApplicable = FALSE;
}


CMatrix4dh& CMatrix4dh::LoadIdentity()
{
	m_col0.m_i = CMathFixed::One;
	m_col0.m_j = 0;
	m_col0.m_k = 0;
	m_col0.m_w = 0;

	m_col1.m_i = 0;							
	m_col1.m_j = CMathFixed::One;	
	m_col1.m_k = 0;
	m_col1.m_w = 0;

	m_col2.m_i = 0;
	m_col2.m_j = 0;
	m_col2.m_k = CMathFixed::One;
	m_col2.m_w = 0;

	m_col3.m_i = 0;
	m_col3.m_j = 0;
	m_col3.m_k = 0;
	m_col3.m_w = CMathFixed::One;

	m_nOfArithmeticOpsSinceLastFixup = 0;

	return *this;
}


CMatrix4dh& CMatrix4dh::LoadIdentity3x3()
{
	m_col0.m_i = CMathFixed::One;
	m_col0.m_j = 0;
	m_col0.m_k = 0;
	m_col0.m_w = 0;

	m_col1.m_i = 0;							
	m_col1.m_j = CMathFixed::One;	
	m_col1.m_k = 0;
	m_col1.m_w = 0;

	m_col2.m_i = 0;
	m_col2.m_j = 0;
	m_col2.m_k = CMathFixed::One;
	m_col2.m_w = 0;

	m_nOfArithmeticOpsSinceLastFixup = 0;

	return *this;
}


CMatrix4dh& CMatrix4dh::Orthonormalize()
{
	CVector3d b0( m_col0.m_i, m_col1.m_i, m_col2.m_i );
	CVector3d b1( m_col0.m_j, m_col1.m_j, m_col2.m_j );

	b0.Normalize();

	fixed dOrtho = b0 * b1;

	if( dOrtho )
	{
		b1.m_i -= CMathFixed::Mul( b0.m_i, dOrtho );
		b1.m_j -= CMathFixed::Mul( b0.m_j, dOrtho );
		b1.m_k -= CMathFixed::Mul( b0.m_k, dOrtho );
	}
		
	b1.Normalize();
	
	CVector3d b2( b0 );

	b2.Cross( b1 );

	m_col0.Set( b0.m_i, b1.m_i, b2.m_i );
	m_col1.Set( b0.m_j, b1.m_j, b2.m_j );
	m_col2.Set( b0.m_k, b1.m_k, b2.m_k );

	m_nOfArithmeticOpsSinceLastFixup = 0;

	return *this;
}


CMatrix4dh& CMatrix4dh::RotateX( fixed degrees )
{
	fixed* a = (fixed*)this;
	fixed a4 = a[4];
	fixed a5 = a[5];
	fixed a6 = a[6];

	fixed cos = CMathFixed::Cos( degrees );
	fixed sin = CMathFixed::Sin( degrees );

	a[4] = CMathFixed::Mul( a4, cos ) + CMathFixed::Mul( a[8], sin );
	a[5] = CMathFixed::Mul( a5, cos ) + CMathFixed::Mul( a[9], sin );
	a[6] = CMathFixed::Mul( a6, cos ) + CMathFixed::Mul( a[10], sin );

	a[8] = CMathFixed::Mul( a4, -sin ) + CMathFixed::Mul( a[8], cos );
	a[9] = CMathFixed::Mul( a5, -sin ) + CMathFixed::Mul( a[9], cos );
	a[10] = CMathFixed::Mul( a6, -sin ) + CMathFixed::Mul( a[10], cos );

	IncrementOperationCount();
	ConsiderOrientationFixup();

	return *this;
}


CMatrix4dh&	CMatrix4dh::RotateY( fixed degrees )
{
	fixed* a = (fixed*)this;
	fixed a0 = a[0];
	fixed a1 = a[1];
	fixed a2 = a[2];

	fixed cos = CMathFixed::Cos( degrees );
	fixed sin = CMathFixed::Sin( degrees );

	a[0] = CMathFixed::Mul( a0, cos ) + CMathFixed::Mul( a[8], -sin );
	a[1] = CMathFixed::Mul( a1, cos ) + CMathFixed::Mul( a[9], -sin );
	a[2] = CMathFixed::Mul( a2, cos ) + CMathFixed::Mul( a[10], -sin );

	a[8] = CMathFixed::Mul( a0, sin ) + CMathFixed::Mul( a[8], cos );
	a[9] = CMathFixed::Mul( a1, sin ) + CMathFixed::Mul( a[9], cos );
	a[10] = CMathFixed::Mul( a2, sin ) + CMathFixed::Mul( a[10], cos );

	IncrementOperationCount();
	ConsiderOrientationFixup();

	return *this;
}

 
CMatrix4dh&	CMatrix4dh::RotateZ( fixed degrees )
{
	fixed* a = (fixed*)this;
	fixed a0 = a[0];
	fixed a1 = a[1];
	fixed a2 = a[2];

	fixed cos = CMathFixed::Cos( degrees );
	fixed sin = CMathFixed::Sin( degrees );

	a[0] = CMathFixed::Mul( a0, cos ) + CMathFixed::Mul( a[4], sin );
	a[1] = CMathFixed::Mul( a1, cos ) + CMathFixed::Mul( a[5], sin );
	a[2] = CMathFixed::Mul( a2, cos ) + CMathFixed::Mul( a[6], sin );

	a[4] = CMathFixed::Mul( a0, -sin ) + CMathFixed::Mul( a[4], cos );
	a[5] = CMathFixed::Mul( a1, -sin ) + CMathFixed::Mul( a[5], cos );
	a[6] = CMathFixed::Mul( a2, -sin ) + CMathFixed::Mul( a[6], cos );

	IncrementOperationCount();
	ConsiderOrientationFixup();

	return *this;
}


CMatrix4dh&	CMatrix4dh::Rotate( fixed degrees, fixed i, fixed j, fixed k )
{
	CMatrix4dh B;
	fixed* b = (fixed*)&B;
	fixed s = CMathFixed::Sin( degrees );
	fixed omc = CMathFixed::One - CMathFixed::Cos( degrees );
	fixed i2 = CMathFixed::Mul( i, i );
	fixed j2 = CMathFixed::Mul( j, j );
	fixed k2 = CMathFixed::Mul( k, k );
	fixed ij = CMathFixed::Mul( i, j );
	fixed ik = CMathFixed::Mul( i, k );
	fixed jk = CMathFixed::Mul( j, k );	
	fixed si = CMathFixed::Mul( s, i );
	fixed sj = CMathFixed::Mul( s, j );
	fixed sk = CMathFixed::Mul( s, k );
	fixed omcij = CMathFixed::Mul( omc, ij ); 
	fixed omcik = CMathFixed::Mul( omc, ik ); 
	fixed omcjk = CMathFixed::Mul( omc, jk ); 

	b[0] = CMathFixed::One + CMathFixed::Mul( omc, -k2-j2 );
	b[1] = sk + omcij;
	b[2] = -sj + omcik;
	b[3] = 0;

	b[4] = -sk + omcij;
	b[5] = CMathFixed::One + CMathFixed::Mul( omc, -k2-i2 );
	b[6] = si + omcjk;
	b[7] = 0;

	b[8] = sj + omcik;
	b[9] = -si + omcjk;
	b[10] = CMathFixed::One + CMathFixed::Mul( omc, -j2-i2 );
	b[11] = 0;

	b[12] = 0;
	b[13] = 0;
	b[14] = 0;

	*this *= B;

	IncrementOperationCount();
	ConsiderOrientationFixup();

	return *this;
}


CMatrix4dh&	CMatrix4dh::Translate( fixed i, fixed j, fixed k ) 
{ 
	m_col3 += CVector4dh( i, j, k );

	m_orientationFixupIsApplicable = FALSE;

	return *this; 
}


CMatrix4dh& CMatrix4dh::LookAtRHUpJ( fixed i, fixed j, fixed k )
{
	CVector3d l( i, j, k );
	l.Normalize();

	m_col0.m_k = l.m_i;
	m_col1.m_k = l.m_j;
	m_col2.m_k = l.m_k;

	CVector3d v;
	fixed Upj = CMathFixed::Mul( l.m_j, l.m_j );

	if( Upj < CMatrix4dh::LookAtUpVectorTolerance )
	{
		v.m_j = CMathFixed::One - Upj;
		v.m_i = -CMathFixed::Mul( l.m_j, l.m_i );
		v.m_k = -CMathFixed::Mul( l.m_j, l.m_k );

		v.Normalize();

		m_col0.m_j = v.m_i;
		m_col1.m_j = v.m_j;
		m_col2.m_j = v.m_k;

		v.Cross( l );

		m_col0.m_i = v.m_i;
		m_col1.m_i = v.m_j;
		m_col2.m_i = v.m_k;	
	}
	else
	{
		v.m_i = CMathFixed::One - CMathFixed::Mul( l.m_i, l.m_i );
		v.m_j = -CMathFixed::Mul( l.m_i, l.m_j );
		v.m_k = -CMathFixed::Mul( l.m_i, l.m_k );

		v.Normalize();

		m_col0.m_i = v.m_i;
		m_col1.m_i = v.m_j;
		m_col2.m_i = v.m_k;

		l.Cross( v );

		m_col0.m_j = l.m_i;
		m_col1.m_j = l.m_j;
		m_col2.m_j = l.m_k;
	}

	m_col0.m_w = 0;
	m_col1.m_w = 0;
	m_col2.m_w = 0;
	m_col3.m_w = CMathFixed::One;
	
	m_nOfArithmeticOpsSinceLastFixup = 0;

	return *this;
}


CMatrix4dh& CMatrix4dh::LookAtRHUpK( fixed i, fixed j, fixed k )
{
	CVector3d l( i, j, k );
	l.Normalize();

	m_col0.m_j = l.m_i;
	m_col1.m_j = l.m_j;
	m_col2.m_j = l.m_k;

	CVector3d v;
	fixed Upk = CMathFixed::Mul( l.m_k, l.m_k );

	if( Upk < CMatrix4dh::LookAtUpVectorTolerance )
	{
		v.m_i = -CMathFixed::Mul( l.m_k, l.m_i );
		v.m_j = -CMathFixed::Mul( l.m_k, l.m_j );
		v.m_k = CMathFixed::One - Upk;

		v.Normalize();

		m_col0.m_k = v.m_i;
		m_col1.m_k = v.m_j;
		m_col2.m_k = v.m_k;

		l.Cross( v );

		m_col0.m_i = l.m_i;
		m_col1.m_i = l.m_j;
		m_col2.m_i = l.m_k;	
	}
	else
	{
		v.m_i = CMathFixed::One - CMathFixed::Mul( l.m_i, l.m_i );
		v.m_j = -CMathFixed::Mul( l.m_i, l.m_j );
		v.m_k = -CMathFixed::Mul( l.m_i, l.m_k );

		v.Normalize();

		m_col0.m_i = v.m_i;
		m_col1.m_i = v.m_j;
		m_col2.m_i = v.m_k;

		v.Cross( l );

		m_col0.m_k = v.m_i;
		m_col1.m_k = v.m_j;
		m_col2.m_k = v.m_k;
	}

	m_col0.m_w = 0;
	m_col1.m_w = 0;
	m_col2.m_w = 0;
	m_col3.m_w = CMathFixed::One;

	m_nOfArithmeticOpsSinceLastFixup = 0;

	return *this;
}


//CMatrix4dh&	CMatrix4dh::Transpose3x3()
//{
//	fixed* a = (fixed*)this;
//	fixed a1 = a[1];
//	fixed a2 = a[2];
//	fixed a3 = a[3];
//	fixed a6 = a[6];
//
//	a[1] = a[4]; a[2] = a[8];
//
//	a[4] = a1; a[6] = a[9];
//
//	a[8] = a2; a[9] = a6;
//
//	//+jmb: could use exclusive-or to swap
//
//	return *this;
//}


CMatrix4dh& CMatrix4dh::SetIgnoreColumn3( const CMatrix4dh& m )
{
	m_col0 = m.m_col0;
	m_col1 = m.m_col1;
	m_col2 = m.m_col2;

	m_orientationFixupIsApplicable = m.m_orientationFixupIsApplicable;
	m_nOfArithmeticOpsSinceLastFixup = m.m_nOfArithmeticOpsSinceLastFixup;
	m_nOfArthmeticOpsTriggeringOrientationFixup =  m.m_nOfArthmeticOpsTriggeringOrientationFixup;

	return *this;
}


CMatrix4dh& CMatrix4dh::operator*=( const CMatrix4dh& b )
{
	fixed* a = (fixed*)this;
	fixed a0 = a[0];
	fixed a1 = a[1];
	fixed a2 = a[2];
	fixed a4 = a[4];
	fixed a5 = a[5];
	fixed a6 = a[6];
	fixed a8 = a[8];
	fixed a9 = a[9];
	fixed a10 = a[10];

	a[0] = CMathFixed::Mul( a0, b[0] ) + CMathFixed::Mul( a4, b[1] ) + CMathFixed::Mul( a8, b[2] );
	a[1] = CMathFixed::Mul( a1, b[0] ) + CMathFixed::Mul( a5, b[1] ) + CMathFixed::Mul( a9, b[2] );
	a[2] = CMathFixed::Mul( a2, b[0] ) + CMathFixed::Mul( a6, b[1] ) + CMathFixed::Mul( a10, b[2] );

	a[4] = CMathFixed::Mul( a0, b[4] ) + CMathFixed::Mul( a4, b[5] ) + CMathFixed::Mul( a8, b[6] );
	a[5] = CMathFixed::Mul( a1, b[4] ) + CMathFixed::Mul( a5, b[5] ) + CMathFixed::Mul( a9, b[6] );
	a[6] = CMathFixed::Mul( a2, b[4] ) + CMathFixed::Mul( a6, b[5] ) + CMathFixed::Mul( a10, b[6] );

	a[8] = CMathFixed::Mul( a0, b[8] ) + CMathFixed::Mul( a4, b[9] ) + CMathFixed::Mul( a8, b[10] );
	a[9] = CMathFixed::Mul( a1, b[8] ) + CMathFixed::Mul( a5, b[9] ) + CMathFixed::Mul( a9, b[10] );
	a[10] = CMathFixed::Mul( a2, b[8] ) + CMathFixed::Mul( a6, b[9] ) + CMathFixed::Mul( a10, b[10] );

	a[12] += CMathFixed::Mul( a0, b[12] ) + CMathFixed::Mul( a4, b[13] ) + CMathFixed::Mul( a8, b[14] );
	a[13] += CMathFixed::Mul( a1, b[12] ) + CMathFixed::Mul( a5, b[13] ) + CMathFixed::Mul( a9, b[14] );
	a[14] += CMathFixed::Mul( a2, b[12] ) + CMathFixed::Mul( a6, b[13] ) + CMathFixed::Mul( a10, b[14] );

	IncrementOperationCount();
	ConsiderOrientationFixup();

	return *this;
}


CMatrix4dh operator*( const CMatrix4dh& a, const CMatrix4dh& b )
{
	return CMatrix4dh( CMathFixed::Mul( a[0], b[0] ) + CMathFixed::Mul( a[4], b[1] ) + CMathFixed::Mul( a[8], b[2] ),
					   CMathFixed::Mul( a[1], b[0] ) + CMathFixed::Mul( a[5], b[1] ) + CMathFixed::Mul( a[9], b[2] ),
					   CMathFixed::Mul( a[2], b[0] ) + CMathFixed::Mul( a[6], b[1] ) + CMathFixed::Mul( a[10], b[2] ),
	
					   CMathFixed::Mul( a[0], b[4] ) + CMathFixed::Mul( a[4], b[5] ) + CMathFixed::Mul( a[8], b[6] ),
					   CMathFixed::Mul( a[1], b[4] ) + CMathFixed::Mul( a[5], b[5] ) + CMathFixed::Mul( a[9], b[6] ),
					   CMathFixed::Mul( a[2], b[4] ) + CMathFixed::Mul( a[6], b[5] ) + CMathFixed::Mul( a[10], b[6] ),

					   CMathFixed::Mul( a[0], b[8] ) + CMathFixed::Mul( a[4], b[9] ) + CMathFixed::Mul( a[8], b[10] ),
					   CMathFixed::Mul( a[1], b[8] ) + CMathFixed::Mul( a[5], b[9] ) + CMathFixed::Mul( a[9], b[10] ),
					   CMathFixed::Mul( a[2], b[8] ) + CMathFixed::Mul( a[6], b[9] ) + CMathFixed::Mul( a[10], b[10] ),

					   CMathFixed::Mul( a[0], b[12] ) + CMathFixed::Mul( a[4], b[13] ) + CMathFixed::Mul( a[8], b[14] ) + a[12],
					   CMathFixed::Mul( a[1], b[12] ) + CMathFixed::Mul( a[5], b[13] ) + CMathFixed::Mul( a[9], b[14] ) + a[13],
					   CMathFixed::Mul( a[2], b[12] ) + CMathFixed::Mul( a[6], b[13] ) + CMathFixed::Mul( a[10], b[14] ) + a[14],
					   
					   a.GetNumOfArithmeticOpsTriggeringOrientationFixup() );

}


inline
void CMatrix4dh::IncrementOperationCount()
{
	if( m_nOfArithmeticOpsSinceLastFixup < MAX_UINT16 )
		m_nOfArithmeticOpsSinceLastFixup++;
}


inline
void CMatrix4dh::ConsiderOrientationFixup()
{
	if( m_orientationFixupIsApplicable &&
		m_nOfArithmeticOpsSinceLastFixup >= m_nOfArthmeticOpsTriggeringOrientationFixup )
		Orthonormalize();
}
